//
// Created by jg on 2021/3/5.
//

#ifndef PATH_TEST_BLH2LOCPOS_HPP
#define PATH_TEST_BLH2LOCPOS_HPP

namespace MAP_GEN {
    const double PI = 3.141592653589793;
    const double D2R = PI/180;
    const double R2D = 180/PI;
    const double RA  = 6378137.0;              //长半轴
    const double E2  = 0.00669437999013;       //偏心率平方

    class BLH2locpos{
    protected:
        Eigen::Vector3d original_point_;
        double resolution_;

        void radiusmn(const double& lat, double& Rm, double& Rn) {
            Rm = RA * (1 - E2) / pow((1 - E2 * sin(lat) * sin(lat)), 1.5);
            Rn = RA / sqrt(1 - E2 * sin(lat) * sin(lat));
        }
        // _angle in [0, 2 PI]
        void angleGet(const double& _x, const double& _y, double& _angle) {
            _angle = atan2(_y, _x);
            if (_angle > 2 * M_PI) {
                _angle -= 2 * M_PI;
            } else if (_angle < 0) {
                _angle += 2 * M_PI;
            }
        }
    public:
        BLH2locpos(){
            original_point_ << 30.528628340918, 114.35572874791, 22.813257;
            original_point_[0] *= D2R;
            original_point_[1] *= D2R;

            resolution_ = 0.2;
        }

        ~BLH2locpos(){}

        void setOriginal(const Eigen::Vector3d& _original){original_point_ = _original;}
        void setResolution(double _resolution){resolution_ = _resolution;}
        Eigen::Vector3d getOriginal(){return original_point_;}

        /***********BLH(rad, rad, m) to loc_xy(m, m)*****************/
        void BLH2xy(const Eigen::Vector3d& loc_BLH, Eigen::Vector3d& loc_xy){
            double Rm, Rn, dn, de, distance, angle;
            radiusmn(original_point_[0], Rm, Rn);
            //北向距离，东向距离
//        std::cout << "BLH :" << loc_BLH[0]*R2D << "," << original_point_[0]*R2D
//        << "\n\t" << loc_BLH[1]*R2D << "," << original_point_[1]*R2D << std::endl;

            dn = (loc_BLH[0] - original_point_[0]) * (Rm + original_point_[2]);
            de = (loc_BLH[1] - original_point_[1]) * (Rn + original_point_[2]) * cos(original_point_[0]);
            distance = sqrt(de * de + dn * dn);

            //计算与x轴夹角，范围为0~2PI,坐标轴x朝北，y朝西
            angleGet(dn, -de, angle);
//        angle = -angle;
            //换到地图的像素坐标
            loc_xy[0] = distance * cos(angle);
            loc_xy[1] = distance * sin(angle);
            loc_xy[2] = 0.01;
        }
        /************loc_xy(m, m) to BLH(lat, lon, alt, --deg, deg, m)****************/
        void XY2BLH(const Eigen::Vector3d& loc_xy, Eigen::Vector3d& loc_BLH){
            double Rm, Rn;
            radiusmn(original_point_[0], Rm, Rn);
            double dn = loc_xy[0];
            double de = -loc_xy[1];

            loc_BLH[0] = (dn / (Rm + original_point_[2]) + original_point_[0]) * R2D;
            loc_BLH[1] = (de / cos(original_point_[0]) / (Rn + original_point_[2]) + original_point_[1]) * R2D;
            loc_BLH[2] = original_point_[2];
        }
    };
}



#endif //PATH_TEST_BLH2LOCPOS_HPP
